//
// Created by hylre on 24-7-4.
//
#include <iostream>
#include <vector>
#include <opencv2/opencv.hpp>

using namespace std;
using namespace cv;

int main() {
    // 准备棋盘格的网格尺寸
    Size patternSize(9, 6);  // 棋盘格内角点的行数和列数

    // 准备棋盘格的物理坐标
    float squareSize = 1.0f;  // 每个棋盘格的大小（单位：任意长度单位）
    vector<Point3f> objectPoints;
    for (int i = 0; i < patternSize.height; ++i) {
        for (int j = 0; j < patternSize.width; ++j) {
            objectPoints.push_back(Point3f(j * squareSize, i * squareSize, 0));
        }
    }

    // 存储棋盘格图像的对象点和图像点
    vector<vector<Point3f>> objectPointsVec;
    vector<vector<Point2f>> imagePointsVec;

    // 读取图像文件
    vector<String> images;
    glob("calibration_images/*.jpg", images);

    Mat img, gray;
    vector<Point2f> corners;
    bool patternFound;

    // 遍历所有图像
    for (size_t i = 0; i < images.size(); ++i) {
        img = imread(images[i]);
        cvtColor(img, gray, COLOR_BGR2GRAY);

        // 查找棋盘格角点
        patternFound = findChessboardCorners(gray, patternSize, corners, CALIB_CB_ADAPTIVE_THRESH | CALIB_CB_NORMALIZE_IMAGE);

        // 如果找到棋盘格角点，添加对象点和图像点
        if (patternFound) {
            objectPointsVec.push_back(objectPoints);
            cornerSubPix(gray, corners, Size(11, 11), Size(-1, -1), TermCriteria(TermCriteria::EPS + TermCriteria::COUNT, 30, 0.1));
            imagePointsVec.push_back(corners);

            // 可视化角点
            drawChessboardCorners(img, patternSize, corners, patternFound);
            imshow("Chessboard Corners", img);
            waitKey(500);
        }
    }

    destroyAllWindows();

    // 标定摄像机
    Mat cameraMatrix, distCoeffs;
    vector<Mat> rvecs, tvecs;
    double rms = calibrateCamera(objectPointsVec, imagePointsVec, img.size(), cameraMatrix, distCoeffs, rvecs, tvecs);

    // 打印结果
    cout << "内部参数矩阵:" << endl;
    cout << cameraMatrix << endl << endl;
    cout << "畸变系数:" << endl;
    cout << distCoeffs << endl;

    return 0;
}
